/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef SKYWALKER_H
#define SKYWALKER_H

#include "Object.h"
#include "Shapes.h"
#include "URG.h"
#include "Sick.h"
#include "Camera.h"

#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlServer.hpp"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlTypes.hpp"
#include "../MaCI/interfaces/Position/PositionServer.hpp"
#include "../MaCI/interfaces/Position/PositionTypes.hpp"
#include "../MaCI/interfaces/Ranging/RangingServer.hpp"
#include "../MaCI/interfaces/Ranging/RangingTypes.hpp"

#define GIMI_CLIENT_API 20000
#include "gimi.h"

#include "GIMIConnector.h"

/*
Class defining ode model for Skywalker robot. 
dimensions for model are given in Skywalker_settings.h
*/

class CSkywalker : public CObject
{
  public:
    CSkywalker(CWorld* World, float x, float y, float heading, std::string name,
               std::string MaCIGroup);
    ~CSkywalker();

    void Activate();

    void Update(float StepSize);

    void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2);

    void Draw();
    //set motor speeds
    void SetSpeed(float speed, float angularSpeed, float acceleration);//, float angularAcceleration);
    //set odometry position
    void SetOdoPosition(float x, float y, float heading);

    static CObject* LoadMLMA(CWorld *world, xmlNode *node);
    
  private:
    
    CCylinder *Upper_cylinder; //the body of robot
    CCylinder *Lower_cylinder; //bumper, attached to upper cylinders body
    CCylinder *RWheel, *LWheel;
    CSphere *FrontCaster, *RearCaster;

    /*this is an invisible ball inside hull. this is neede for robot vs robot
    collisions as ode doesn't provide cylinder vs cylinder collision check */
    //CSphere *collider;

    dJointID RMotor, LMotor, FCasterJoint, RCasterJoint;
    //CURG *LaserScanner;
    CSick *LaserScanner;
    
    TMaterial WheelMaterial, HullMaterial;
    std::string name;
    std::string MaCIGroup;
    //CCamera *camera;
    
    gimi::GIMI gimi;

    MaCI::MaCICtrl::CMaCICtrlServer mcs;
    gim::binbag::CBinBag binbag;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer;
    MaCI::SpeedCtrl::CSpeedCtrlData SpeedCtrlData;
    MaCI::Position::CPositionServer PositionServer;
    MaCI::Ranging::CRangingData RangingServerData;
    MaCI::Ranging::CRangingServer RangingServer;
    float iSpeed;
    float iAngularSpeed;

    float posX, posY, posZ, heading;
    float transX, transY, transZ, transHeading;
    float odoPosX, odoPosY, odoPosZ, odoHeading;
    //float speed, angularSpeed;

    float laserTimer, positionTimer;

    //these are for checking if bumper has just been released
    bool bumperHit;
    int stepsSinceHit;

    static int refCount;

    void calculateOdoPose();
    
    void connectGIMI();
    void setPose(float x, float y, float z, float heading);
    

};

#endif
